/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include<global_planner/astar.h>
#include<costmap_2d/cost_values.h>

namespace global_planner {
/**
 * 这段代码实现了A*算法中的一个函数calculatePotentials()和一个辅助函数add()，用于计算从起点到终点的最短路径。
 * sukai
 * @param p_calc
 * @param xs
 * @param ys
 */
AStarExpansion::AStarExpansion(PotentialCalculator* p_calc, int xs, int ys) :
        Expander(p_calc, xs, ys) {
}
//costmap_->getCharMap() = costs；   /home/sukai/workspace/xju-robot_gai/src/thirdparty/navigation/global_planner/src/planner_core.cpp 479
    bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
                                        int cycles, float* potential) {//potential = float* potential_array_; 该函数的作用是计算出从起点到终点的所有路径的代价值，即potential_array_数组
    queue_.clear();//清空一个存放优先级队列的vector容器queue_。
    int start_i = toIndex(start_x, start_y);//计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
    queue_.push_back(Index(start_i, 0));//将起点压入优先级队列，优先级为0。//将起点 start_i 加入到优先队列 queue_ 中，第二个参数 0 表示起点的势能 代价值为 0。//将势能数组 potential 的所有元素都赋值为 POT_HIGH，表示所有未探索的空间的势能都是无穷大。

    std::fill(potential, potential + ns_, POT_HIGH);//初始化一个数组potential，用于存储每个格子的路径潜力值，将其全部赋值为一个较大的值POT_HIGH。 #define POT_HIGH 1.0e10 表示所有未探索的空间的势能都是无穷大。
    potential[start_i] = 0;//将起点的路径潜力值设置为0。// 将起点的势能设为 0。

    int goal_i = toIndex(end_x, end_y);//计算终点在一维数组中的下标。 //将终点坐标 end_x 和 end_y 转化为一维数组中的索引 goal_i。
    int cycle = 0;  //设置循环次数 cycle 为 0。
// nx * ny * 2  = cycles
/**
 *     nx * ny * 2  = cycles
 *     这个参数是用来指定potential_array_数组的大小的，这个数组是用来存储每个点到目标点的距离的。在这个函数中，potential_array_数组的大小是nx * ny * 2，
             其中nx和ny是地图的宽度和高度，2是因为在计算距离时，需要同时计算正向和反向的距离，因此数组的大小需要乘以2。在这个函数中，nx和ny的值是通过costmap_的getSizeInCellsX()和getSizeInCellsY()函数获取的，
             这两个函数返回的是地图的宽度和高度，单位是cell。因此，nx * ny * 2实际上是地图中所有cell的数量乘以2。
 */
    while (queue_.size() > 0 && cycle < cycles) {//循环条件为队列不为空且循环次数小于cycles=nx * ny * 2 。 // 当优先队列不为空且循环次数小于 cycles 时，进行循环。
        Index top = queue_[0];//Index top = queue_[0];：获取队列中优先级最高的元素。 // 取出优先队列中的第一个元素（最小势能值），保存到 top 中。
        std::pop_heap(queue_.begin(), queue_.end(), greater1());//将队列中的元素重新排列，使得优先级最高的元素被移到最后一个位置。 //弹出优先队列中的最小元素，并保持堆的性质。
        queue_.pop_back();//：弹出队列中的最后一个元素，即优先级最高的元素。 // 删除队列的最后一个元素。

        int i = top.i;//获取弹出的元素的下标。//取出 top 中保存的元素的索引值 i。
        if (i == goal_i)  //如果弹出的元素是终点，则返回true表示找到了最短路径。 //如果当前位置 i 是终点，则找到了一条通路，直接返回 true。
            return true;
//costmap_->getCharMap() = costs；
        add(costs, potential, potential[i], i + 1, end_x, end_y); //根据当前元素的下标，计算其相邻格子的路径潜力值并压入队列。// 调用 add 函数，将 i 的邻居节点 i+1 加入到队列中。
        add(costs, potential, potential[i], i - 1, end_x, end_y);
        add(costs, potential, potential[i], i + nx_, end_x, end_y);
        add(costs, potential, potential[i], i - nx_, end_x, end_y);

        cycle++;//循环次数加1。//周期数加1，表示已经处理了一个周期的数据。
    }

    return false;//循环结束后返回false，表示没有找到最短路径。//如果执行到这里，说明队列已经空了（即所有的点都被处理过了），但还没有找到终点。此时返回false，表示没有找到合法的路径。
}
// 添加一个新的节点到搜索队列。 //costmap_->getCharMap() = costs；
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
                         int end_y) {
    if (next_i < 0 || next_i >= ns_)//如果下标越界，则直接返回。  ns_ = nx * ny;
        return;

    if (potential[next_i] < POT_HIGH)// 如果这个节点已经被处理过了，也就是它的 potential 值已经被赋值过了，直接返回。 [potential被赋值过了，这个值肯定小于POT_HIGH ，说明这个点已经被处理过了，不需要再处理了。 ]
        return;
   //costmap_->getCharMap() = costs；   lethal_cost_ = 253 致命代价值
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))//NO_INFORMATION = 255 未知 保留给没有足够信息的单元格。；如果该点的cost值超过lethal_cost_，或者未知但unknown_标志为false，也直接返回
        return;

    /**
        potential 可以翻译为“势能”，在该程序中用来表示从起点到每个点的估计距离。在 A* 算法中，估价函数 f(n) = g(n) + h(n) 中，potential 即为 h(n)，即当前节点 n 到目标节点的估计距离。
                由于使用了 A* 算法，因此需要预估每个点到目标节点的距离，即计算每个点的 potential 值。
      --------------------------
     lethal_cost = 253  //致命代价值

     neutral_cost = 66 //中等代价值

     cost_factor = 0.55 //代价地图与每个代价值相乘的因子

原文链接：https://blog.csdn.net/yaked/article/details/125503202

      这个函数需要传入当前已计算的所有格子的潜在值（potential 预估每个点到目标节点的距离）、该格子的代价值（cost）、该格子的索引（n）以及前一个格子的潜在值（prev_potential）。其中，prev_potential 是一个可选的参数，如果没有传入，
         则会计算该格子周围四个邻居格子的最小潜在值作为 prev_potential。这是一种较为简单的潜在值计算方法。
        在函数内部，首先通过 n 计算出该格子的横向邻居格子和纵向邻居格子的潜在值的最小值，即 min_h 和 min_v。然后，将 min_h 和 min_v 中的最小值作为 prev_potential，加上 cost，就得到了该格子的潜在值。
        这个函数的具体实现方式可能会因具体应用场景而有所差异，但是通常都需要考虑当前格子的代价以及周围格子的潜在值等因素。

    */
     //由于使用了 A* 算法，因此需要预估每个点到目标节点的距离，即计算每个点的 potential（预估每个点到目标节点的距离） 值。 // calculatePotential 这段代码是一个函数，用于计算一个格子的潜在（potential）值，即到达该格子的预估代价。
        //costmap_->getCharMap() = costs；   /home/sukai/workspace/workspace_ros_car_noetic/src/navigation-noetic/global_planner/include/global_planner/potential_calculator.h  63行
    potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);//计算这个点的potential（预估每个点到目标节点的距离）值。 //potential = float* potential_array_;

    int x = next_i % nx_, y = next_i / nx_;//计算当前点到终点的曼哈顿距离。  ns_ = nx * ny;
   // float distance = abs(end_x - x) + abs(end_y - y);//计算当前点到终点的曼哈顿距离。
    
       //苏凯 欧式距离
    float distance =sqrt( pow(end_x - x,2) + pow(end_y - y,2) )+(abs(end_y - y)/2);//2024-02-22
    
    //potential（预估每个点到目标节点的距离） 值
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_));//将这个节点放到搜索队列里，并计算该节点的priority（potential值+距离）。
    std::push_heap(queue_.begin(), queue_.end(), greater1());//调整搜索队列，使之满足小根堆的性质，即priority小的节点在队列前面。
}

} //end namespace global_planner




